import numpy as np
import time
from trajectories.traj_base import BaseTrajectory
from utils.b_spline_geomdl import approximate_b_spline_path, approximate_b_spline


class ForwardTrajectory(BaseTrajectory):
    TRACK_KEY = "Forward"

    def __init__(self):
        super().__init__()

    def get_track_key(self):
        return self.TRACK_KEY

    def get_color(self):
        return [123, 46, 231]

    def tracking(self, args, update_cb, ls):
        angles_list = args[0]
        # print("angles_list: ", angles_list)

        slice_count = len(angles_list)
        slice_time_second = (1 / slice_count) * 0.3

        while True:
            for angle in angles_list:

                if not self.is_running:
                    return False

                alpha, beta = angle
                if alpha == 999 or beta == 999:
                    continue

                if ls.try_update_forward(alpha, beta):
                    self._add_angles(angle)
                    self._add_point(ls.get_current_end_point())
                    update_cb()
                    time.sleep(slice_time_second)

            self.is_walking_completed = True
